#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include "Pwm.h"
#include "MOVE.H"
#include "PID.h"
#include "TIM.h"

extern int16_t Serial_RxPacket[];
uint16_t count;

void Timer_Init(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);			
	
	TIM_InternalClockConfig(TIM2);		
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;				
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;		
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;	
	TIM_TimeBaseInitStructure.TIM_Period = 20000 - 1;				
	TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1;				
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;			
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);					
	
	TIM_ClearFlag(TIM2, TIM_FLAG_Update);						
																
	TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);					
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);																																			
	NVIC_InitTypeDef NVIC_InitStructure;						
	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;				
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;				
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;			
	NVIC_Init(&NVIC_InitStructure);								
	
	TIM_Cmd(TIM2, ENABLE);			
}

/****************************PID******************************************/

/***************************??************************************/
int pre_X_PWM = 1500;
int pre_Y_PWM = 1500;

void TIM2_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
	{
		int err_X_PWM = X_V_PID_Value(Serial_RxPacket[1]-160,0);
		int now_X_PWM = pre_X_PWM + err_X_PWM;
		int err_Y_PWM = Y_V_PID_Value(Serial_RxPacket[2]-120,0);
		int now_Y_PWM = pre_Y_PWM - err_Y_PWM;
/*********************************************XXX***************************/
		if(now_X_PWM>=2600) 
		{
			now_X_PWM=2600;
			X_Pwm(2600);
		}
		else if(now_X_PWM<=400) 
		{
			now_X_PWM =400;
			X_Pwm(400);
		}
		else
		{
			X_Pwm(now_X_PWM);
		}
	/**********************************YYY***************************************************/
		if(now_Y_PWM>=2600) 
		{
			now_Y_PWM=2600;
			Y_Pwm(2600);
		}
		else if(now_Y_PWM<=1100) 
		{
			now_Y_PWM =1100;
			Y_Pwm(1100);
		}
		else
		{
			Y_Pwm(now_Y_PWM);
		}
			pre_X_PWM = now_X_PWM;
		  pre_Y_PWM = now_Y_PWM;
	}
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}